#include <ros/ros.h>

#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h>

// -------------need back service--------------
struct Point2D{
    double x, y;
    Point2D(double x_, double y_){
        this->x = x_;
        this->y = y_;
    }
    Point2D(){this->x = 0; this->y = 0;}

    Point2D& operator =(const Point2D& obj){
        this->x = obj.x;
        this->y = obj.y;
        return *this;
    }

    Point2D& operator -(const Point2D& obj){
        this->x -= obj.x;
        this->y -= obj.y;
        return *this;
    }

    double norm(){
        return sqrt(this->x * this->x + this->y * this->y);
    }
};

struct Point3D{
    double x, y, z;
    Point3D(double x_, double y_, double z_){
        this->x = x_;
        this->y = y_;
        this->z = z_;
    }
    Point3D(){this->x = 0; this->y = 0; this->z = 0;}

    Point3D& operator = (const Point3D& obj){
        this->x = obj.x;
        this->y = obj.y;
        this->z = obj.z;
        return *this;
    }
};

double crossProduct(const Point2D& A, const Point2D& B, const Point2D& C) {
    return (B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x);
}

// for rmul
std::vector<Point2D> home1 = 
    {Point2D(1, 1.5), Point2D(1, -1.5), Point2D(-1, -1.5), Point2D(-1., 1.5)};
std::vector<Point2D> home2 =
    {Point2D(1., 1.5), Point2D(1., -1.5), Point2D(-1., -1.5), Point2D(-1., 1.5)};
std::vector<std::vector<Point2D>> HomeList{home1, home2};


// -------------tf tree service--------------

tf::Vector3 radar_to_base_tvec_xyz = tf::Vector3(-0.13, 0., 0.);
tf::Vector3 radar_to_low_tvec_xyz = tf::Vector3(-0.184, 0.19, 0.105);
tf::Vector3 radar_to_high_tvec_xyz = tf::Vector3(-0.184, -0.19, 0.265);



// -------------location service--------------
namespace location {

struct Area{
    Point2D center;
    std::vector<Point2D> corners;
    double expected_chassis_rad;

    Area(const Point2D& center_, const std::vector<Point2D>& corners_, double expected_chassis_rad_){
        this->center = center_;
        this->corners = corners_;
        this->expected_chassis_rad = expected_chassis_rad_;
    }
};

}
// enum UpSlopeAreaList{

//     MY_UPSLOPE_BASE_RIGHT = 0, 
//     MY_UPSLOPE_BASE_LEFT = 1, 
//     MY_UPSLOPE_CIRCLE_RIGHT = 2, 
//     MY_UPSLOPE_CIRCLE_LEFT = 3, 

//     ENEMY_UPSLOPE_BASE_RIGHT = 4, 
//     ENEMY_UPSLOPE_BASE_LEFT = 5, 
//     ENEMY_UPSLOPE_CIRCLE_RIGHT = 6, 
//     ENEMY_UPSLOPE_CIRCLE_LEFT = 7
// };


// my view angle
// Area my_upslope_base_right = Area (Point2D(-1.45, -5), std::vector<Point2D>{Point2D(-2.28, -3.52), Point2D(-0.8, -3.52), Point2D(-0.8, -6), Point2D(-2.17, -6)}, -M_PI / 2);
// Area my_upslope_base_left = Area (Point2D(-1.68, 3.35), std::vector<Point2D>{Point2D(-2.6, 4.15), Point2D(-0.12, 4.15), Point2D(-0.73, 2.72), Point2D(-2.76, 2.72)}, M_PI / 2);

// Area my_upslope_circle_right = Area (Point2D(4.75, -1.6), std::vector<Point2D>{Point2D(3.3, -0.64), Point2D(4.74, -0.22), Point2D(6.41, -2.64), Point2D(4.82, -3)}, 3 * M_PI / 4);
// Area my_upslope_circle_left = Area (Point2D(6.36, 4.9), std::vector<Point2D>{Point2D(6.5, -6.27), Point2D(7.5, 5.5), Point2D(6.26, 3.64), Point2D(5.13, 4.37)}, -3 * M_PI / 4);

// //enemy view angle
// Area enemy_upslope_base_right = Area (Point2D(18.4, 5), std::vector<Point2D>{Point2D(17.8, 3.52), Point2D(19.13, 6), Point2D(19.13, 3.52), Point2D(17.8, 3.52)}, M_PI / 2);
// Area enemy_upslope_base_left = Area (Point2D(18.4, -3.35), std::vector<Point2D>{Point2D(17.9, -2.72), Point2D(19.6, -2.72), Point2D(19.4, -4.15), Point2D(17, -4.15)}, -M_PI / 2);

// Area enemy_upslope_circle_right = Area (Point2D(12.16, 1.6), std::vector<Point2D>{Point2D(10.8, 2.5), Point2D(12, 3), Point2D(13.5, 0.5), Point2D(12.2, 0.1)}, -3 * M_PI / 4);
// Area enemy_upslope_circle_left = Area (Point2D(10.5, -4.9), std::vector<Point2D>{Point2D(10.7, -3.65), Point2D(11.76, -4.31), Point2D(10.46, -6.27), Point2D(9.2, -5.6)}, 3 * M_PI / 4);

// std::vector<Area> UpSlopeAreas = {my_upslope_base_right, my_upslope_base_left, my_upslope_circle_right, my_upslope_circle_left, 
//                                 enemy_upslope_base_right, enemy_upslope_base_left, enemy_upslope_circle_right, enemy_upslope_circle_left };



// enum TinyGimbalPatrolAreaList {
//     MY_BASE = 0,
//     MY_BASE_FRONT_OUTPOST = 1,
//     MY_BASE_FRONT_LEFT = 2,
//     MY_T_HIGH_LAND = 3,
//     MY_CIR_HIGH_LAND = 4,
//     MY_HOSPITAL = 5,

//     CENTER = 6,

//     ENEMY_BASE = 7,
//     ENEMY_BASE_FRONT_OUTPOST = 8,
//     ENEMY_BASE_FRONT_LEFT = 9,
//     ENEMY_T_HIGH_LAND = 10,
//     ENEMY_CIR_HIGH_LAND = 11,
//     ENEMY_HOSPITAL = 12,

//     AREA_UNDEFINED = 13
// };

// Area my_base = Area(Point2D(0, 0),std::vector<Point2D>{Point2D(-5.38, 2.6),Point2D(3.23, 2.7),Point2D(2.94, -1),Point2D(1.91, -2.8),Point2D(-5, -3.2)}, 0);
// Area my_base_front_outpost = Area(Point2D(2.5, -4.1),std::vector<Point2D>{Point2D(-2.5, -4),Point2D(2.94, -1),Point2D(6.45, -2.57),Point2D(7.24, -7.4),Point2D(-2.5, -7.4)}, 0);
// Area my_base_front_left = Area(Point2D(3, 5),std::vector<Point2D>{Point2D(2.45, 7.4),Point2D(6.73, 7.4),Point2D(3.23, 2.7),Point2D(-0.36, 2.7)}, 0);
// Area my_t_high_land = Area(Point2D(0, 5),std::vector<Point2D>{Point2D(-2.56, 7.4),Point2D(2.45, 7.4),Point2D(-0.36, 2.7),Point2D(-2.56, 2.7)}, 0);
// Area my_cir_high_land = Area(Point2D(5, 1.7),std::vector<Point2D>{Point2D(6.73, 7.4),Point2D(9.45, 7.4),Point2D(6.45, -2.57),Point2D(2.94, -1),Point2D(3.23, 2.7)}, 0);
// Area my_hospital = Area(Point2D(-3.75, -5.6),std::vector<Point2D>{Point2D(-5, -3.2),Point2D(-2.5, -4),Point2D(-2.5, -7.4),Point2D(-5, -7.4)}, 0);

// Area center = Area(Point2D(8.5, 0),std::vector<Point2D>{Point2D(9.45, 7.4),Point2D(10.4, 2.6),Point2D(7.24, -7.4),Point2D(6.45, -2.6)}, 0);

// Area enemy_base = Area(Point2D(17.5, 0),std::vector<Point2D>{Point2D(14.1, 1),Point2D(19.3, 3.34),Point2D(21.3, 3.2),Point2D(19.6, -2.6),Point2D(13.8,-2.7)}, 0);
// Area enemy_base_front_outpost = Area(Point2D(14, 4.1),std::vector<Point2D>{Point2D(9.45, 7.4),Point2D(19, 7.4),Point2D(19.3, 3.34),Point2D(14.1, 1),Point2D(10.4, 2.6)}, 0);
// Area enemy_base_front_left = Area(Point2D(14, -5),std::vector<Point2D>{Point2D(13.8, -2.7),Point2D(17.3, -2.7),Point2D(14.6, -7.4),Point2D(9.9, -7.4)}, 0);
// Area enemy_t_high_land = Area(Point2D(18.4, -5),std::vector<Point2D>{Point2D(17.3, -2.7),Point2D(19.6, -2.6),Point2D(19.6, -7.4),Point2D(14.6, -7.4)}, 0);
// Area enemy_cir_high_land = Area(Point2D(12, -1.7),std::vector<Point2D>{Point2D(10.4, 2.6),Point2D(14.1, 1),Point2D(13.8, -2.7),Point2D(9.9,-7.4),Point2D(7.24,-7.4)}, 0);
// Area enemy_hospital = Area(Point2D(20, 5.3),std::vector<Point2D>{Point2D(19, 7.4),Point2D(21.3, 7.4),Point2D(21.3, 3.2),Point2D(19.3, 3.34)}, 0);

// std::vector<Area> PatrolAreas = {my_base, my_base_front_outpost, my_base_front_left, my_t_high_land, my_cir_high_land,
//                     center, enemy_base, enemy_base_front_outpost, enemy_base_front_left, enemy_t_high_land, enemy_cir_high_land, enemy_hospital};



// -----tiny gimbal patrol service-------

// struct AreaPatrolPoints {
//     std::vector<Point3D> high_gimbal_patrol_points;
//     std::vector<Point3D> low_gimbal_patrol_points;

//     AreaPatrolPoints(const std::vector<Point3D>& high, const std::vector<Point3D>& low){
//         this->high_gimbal_patrol_points = high;
//         this->low_gimbal_patrol_points = low;
//     }
//     AreaPatrolPoints(){}

// };

// struct TinyGimbalPatrolData {
//     location::TinyGimbalPatrolAreaList last_location = location::TinyGimbalPatrolAreaList::AREA_UNDEFINED;
//     ros::Time last_location_update_time;

//     double high_gimbal_patrol_remain_time = 2.;
//     int high_gimbal_patrol_index = 0;

//     double low_gimbal_patrol_remain_time = 2.;
//     int low_gimbal_patrol_index = 0;

//     ros::Time patrol_timestamp;
// };

// AreaPatrolPoints my_base_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(-5,1.93,0.2),Point3D(-1.44,5.38,0.6),Point3D(5.26,4.0,0.75),Point3D(3.78,0.45,0.75),Point3D(4.5,-4.5,0.2)},
//                 std::vector<Point3D>{Point3D(-5,1.93,0.2),Point3D(3.08,6.2,0.3),Point3D(4.5,-4.6,0.2)});
// AreaPatrolPoints my_base_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(10,0,0.2)},
//                 std::vector<Point3D>{Point3D(10,0,0.2)});
// AreaPatrolPoints my_base_front_outpost_points =
// AreaPatrolPoints(std::vector<Point3D>{Point3D(4.25,2.1,0.75),Point3D(12.5,-0.74,0.75),Point3D(10.7,-4.5,0.75),Point3D(10.7,-7,0.3),Point3D(4.5,-4.5,0.2)},
//                 std::vector<Point3D>{Point3D(0,0,0.2),Point3D(9.7,-1.9,0.3),Point3D(10,-7,0.3)});
// AreaPatrolPoints my_base_front_left_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(-1.2,5.74,0.6),Point3D(5.7,4.2,0.75),Point3D(13.2,5.6,1.2),Point3D(11.4,3.5,0.3),Point3D(12.6,-1.15,0.75)},
//                 std::vector<Point3D>{Point3D(8.6,0.85,0.2),Point3D(13,3.65,0.3)});
// AreaPatrolPoints my_t_high_land_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(5.7,4.2,0.75),Point3D(13.2,5.6,1.2),Point3D(11.4,3.5,0.3),Point3D(12.6,-1.15,0.75)},
//                 std::vector<Point3D>{Point3D(8.6,0.85,0.2),Point3D(13,3.65,0.3)});
// AreaPatrolPoints my_cir_high_land_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(13,3.5,0.3),Point3D(12.5,-0.35,0.75),Point3D(10.74,-4.36,0.75),Point3D(7,-4.5,0.3)},
//                 std::vector<Point3D>{Point3D(6.9,9,0.3),Point3D(13,3.65,0.3),Point3D(4.23,-3.6,0.75)});
// AreaPatrolPoints my_hospital_points = 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(-2,0,0.3),},
//                 std::vector<Point3D>{Point3D(-2,0,0.3)});

// AreaPatrolPoints center_points =
// AreaPatrolPoints(std::vector<Point3D>{Point3D(14.2,6.6,0.8),Point3D(14.2,6.6,0.2),Point3D(14.8,-0.4,0.75),Point3D(10.4,-7,0.3),Point3D(4.5,-4.3,0.2),Point3D(5.84,7,0.3)},
//                 std::vector<Point3D>{Point3D(5.9,7.35,0.3),Point3D(0.5,1.4,0.2),Point3D(4.7,-4,0.2),Point3D(8.7,-7,0.3),Point3D(15.34,-0.8,0.2),Point3D(10.05,7,0.2)});

// AreaPatrolPoints enemy_base_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(20.55,4.1,0.2),Point3D(18,-5.7,0.2),Point3D(18,-5.7,0.6),Point3D(12,-7,0.3),Point3D(7.9,-0.55,0.75),Point3D(10.5,6.3,0.3),Point3D(15.9,5.2,0.6)},
//                 std::vector<Point3D>{Point3D(10.4,6.3,0.2),Point3D(13.1,-5.6,0.3),Point3D(20.35,3.3,0.2),Point3D(12.1,4.4,0.2)});
// AreaPatrolPoints enemy_base_front_outpost_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(12.3,7.5,0.3),Point3D(17.6,2.2,0.2),Point3D(17.6,2.2,0.6),Point3D(12,-3.5,0.75),Point3D(6.16,6.7,0.3)},
//                 std::vector<Point3D>{Point3D(7.2,6.8,0.3),Point3D(16.1,-0.1,0.2)});
// AreaPatrolPoints enemy_base_front_left_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(14.3,-2.1,0.2),Point3D(17.55,-4.8,0.6),Point3D(17.55,-4.8,0.2),Point3D(12.3,-6.8,0.3),Point3D(11.9,-0.63,0.75)},
//                 std::vector<Point3D>{Point3D(8.85,-3.2,0.2),Point3D(2.3,-6.8,0.3),Point3D(17.55,-4.8,0.2),Point3D(14.3,-2.1,0.2)});
// AreaPatrolPoints enemy_t_high_land_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(18.10,0.8,0.2),Point3D(20.67,-5.3,0.6),Point3D(17.25,-7,0.6),Point3D(13.75,-4.9,0.6)},
//                 std::vector<Point3D>{Point3D(13.75,-4.9,0.6),Point3D(17.25,-7,0.6),Point3D(20.67,-5.3,0.6),Point3D(18.10,0.8,0.6)});
// AreaPatrolPoints enemy_cir_high_land_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(11,4.7,0.3),Point3D(17.7,-0.6,0.2),Point3D(10.74,-6.9,0.3),Point3D(6.4,-0.43,0.3)},
//                 std::vector<Point3D>{Point3D(6.4,-0.43,0.75),Point3D(10.74,-6.9,0.75),Point3D(17.7,-0.6,0.75),Point3D(11,4.7,0.75)});
// AreaPatrolPoints enemy_hospital_points= 
// AreaPatrolPoints(std::vector<Point3D>{Point3D(19.80,6.9,0.2),Point3D(20.3,2.06,0.2)},
//                 std::vector<Point3D>{Point3D(19.74,2.5,0.2),Point3D(19.80,6.9,0.2)});

// std::vector<AreaPatrolPoints> AreaPatrolList = {my_base_points, my_base_front_outpost_points, my_base_front_left_points, my_t_high_land_points, my_cir_high_land_points,
//     my_hospital_points, center_points, enemy_base_points, enemy_base_front_outpost_points, enemy_base_front_left_points, enemy_t_high_land_points, enemy_cir_high_land_points, enemy_hospital_points};

// geometry_msgs::Point enemy_outpost;

// -------------nav service--------------
namespace nav {

enum NavMode {
    PATROLLING = 0,
    CHASEING = 2,
    SINGLE_POINT = 1,
    STOP = -1
};

enum NavPoint {
    MY_HOME_BASE = 0,
    MY_HOME_CIR = 1,
    MY_HOME_BUFF = 2,

    MY_T_HIGH_LAND = 3,
    MY_OUTPOST = 4,
    MY_HOSPITAL = 5,

    ENEMY_OUTPOST = 6,
    ENEMY_BASE = 7,
    ENEMY_T_HIGH_LAND = 8
};

typedef std::vector<Point2D> NavPointList;

NavPointList home_large_patrolling = {Point2D(6.2, 6.9), Point2D(0, 0), Point2D(6.24, -4.25)};
NavPointList home_small_patrolling = {Point2D(0.5, 1), Point2D(-0.5, -1), Point2D(-0.5, 1), Point2D(0.5, -1)};
NavPointList enemy_large_patrolling = {Point2D(13.7, -6.75), Point2D(14.8,-4.2)};
NavPointList enemy_small_patrolling = {Point2D(13.7, -6.75), Point2D(14.8,-4.2)};

NavPointList my_home_base = {Point2D(0, 0)};
NavPointList my_home_cir = {Point2D(5.25, 4)};
NavPointList my_home_buff = {Point2D(0, -3.8)};
NavPointList my_t_high_land = {Point2D(-1.2, 5.43)};
NavPointList my_outpost = {Point2D(6.24, -4.25)};
NavPointList my_hospital = {Point2D(-3.5, -6.53)};

NavPointList enemy_outpost = {Point2D(10.5, 3.4)};
NavPointList enemy_base = {Point2D(16.4, 0)};
NavPointList enemy_t_high_land = {Point2D(17.3, -5.5)};

std::vector<NavPointList> patrolling_list = {home_large_patrolling, home_small_patrolling, enemy_large_patrolling, enemy_small_patrolling};
std::vector<NavPointList> single_point_list = {my_home_base, my_home_cir, my_home_buff, my_t_high_land, my_outpost, my_hospital, enemy_outpost, enemy_base, enemy_t_high_land};



}